
#include <pcl/common/common.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/pcd_io.h>

#include <pcl/console/parse.h>

#include <opencv2/opencv.hpp>

using namespace cv;

using namespace std;
class Contor
{

public:
	float AreaCalculation(pcl::PointCloud<pcl::PointXYZ> sortedpoints)
	{
		float result = 0;
		for (int i = 0; i < sortedpoints.size() - 1; i++)
		{

			float currentx = sortedpoints[i].x;
			float nextx = sortedpoints[i + 1].x;

			float currenty = sortedpoints[i].y;
			float nexty = sortedpoints[i + 1].y;

			float temp = currentx * nexty - nextx * currenty;
			result = result + temp;
		}

		// 最后一个点与第一个点
		float currentx = sortedpoints[sortedpoints.size() - 1].x;
		float nextx = sortedpoints[0].x;

		float currenty = sortedpoints[sortedpoints.size() - 1].y;
		float nexty = sortedpoints[0].y;

		float temp = currentx * nexty - nextx * currenty;
		result = result + temp;
		return abs(result / 2.0);
	}

	void mainh(Mat gray)
	{
		//    resize(im, im, Size(0, 0), 0.5, 0.5);
		Mat im;
		// cvtColor(im, gray, cv::COLOR_BGR2GRAY);
		cvtColor(gray, im, cv::COLOR_GRAY2BGR);

		Mat imBin;
		threshold(gray, imBin, 150, 255, THRESH_BINARY);

		vector<vector<Point>> contours;
		findContours(imBin, contours, RETR_EXTERNAL, CHAIN_APPROX_NONE);

		Mat im1 = im.clone();
		Mat im2 = im.clone();
		for (auto &contour : contours)
		{
			// 最大外接矩阵
			Rect rect = boundingRect(contour);
			rectangle(im, rect, Scalar(0, 0, 255), 1);

			// 最小外接矩形
			RotatedRect rotatedRect = minAreaRect(contour);
			Point2f pts[4];
			rotatedRect.points(pts);
			Point2f pt = rotatedRect.center;
			for (int i = 0; i < 4; ++i)
			{
				if (i == 3)
				{
					line(im1, pts[i], pts[0], Scalar(255, 255, 0), 1);
				}
				else
				{
					line(im1, pts[i], pts[i + 1], Scalar(255, 255, 0), 1);
				}
			}
			circle(im1, pt, 1, Scalar(0, 0, 255), -1);

			// 外接多边形
			Mat ploys;
			approxPolyDP(contour, ploys, 5, true);
			// draw ploy
			Vec2i pt1, pt2;
			for (int i = 0; i < ploys.rows; ++i)
			{
				if (i == ploys.rows - 1)
				{
					pt1 = ploys.at<Vec2i>(i);
					pt2 = ploys.at<Vec2i>(0);
				}
				else
				{
					pt1 = ploys.at<Vec2i>(i);
					pt2 = ploys.at<Vec2i>(i + 1);
				}
				line(im2, pt1, pt2, Scalar(0, 0, 255), 2);
			}
		}

		// imshow("im", im);
		// imshow("im1", im1);
		// imshow("im2", im2);

		// waitKey(0);
		// destroyAllWindows();
	}
	// 使用格网法统计面积
	// 输入：
	//         sortedpoints       待统计的点云
	//         cell_size          格网尺寸
	// 输出：
	//         float              面积
	float statisGrids(pcl::PointCloud<pcl::PointXYZ> sortedpoints, string PATH, float cell_size)
	{

		pcl::PointXYZ min, max;
		pcl::getMinMax3D(sortedpoints, min, max);
		float x_max = max.x + cell_size;
		float x_min = min.x - cell_size;

		float y_max = max.y + cell_size;
		float y_min = min.y - cell_size;

		float myrows = ceil((y_max - y_min) / cell_size);
		float mycols = ceil((x_max - x_min) / cell_size);
		cv::Mat grids(myrows, mycols, CV_8UC1);
		cout << myrows << " " << mycols << " " << cell_size << endl;
		grids.setTo(0);

		for (int i = 0; i < sortedpoints.size(); i++)
		{
			int temp_row = ceil((sortedpoints[i].y - y_min) / cell_size);
			int temp_col = ceil((sortedpoints[i].x - x_min) / cell_size);
			if (grids.at<uint8_t>(temp_row, temp_col) == 0)
				grids.at<uint8_t>(temp_row, temp_col) = 255;
		}

		// 统计不为空的格网
		int numberofunempty = 0;
		for (int i = 0; i < myrows; i++)
		{
			for (int j = 0; j < mycols; j++)
			{
				if (grids.at<uint8_t>(i, j) > 0)
				{
					numberofunempty++;
				}
			}
		}
		mainh(grids);
		cout << grids.rows << endl;
		cv::imwrite(PATH + "/plant.png", grids);
		// cv::imshow("a", grids);
		float result = numberofunempty * cell_size * cell_size * 100.0 * 100.0;
		cv::FileStorage fs(PATH + "/area.yaml", cv::FileStorage::WRITE);
		fs << "area" << result;
		fs.release();
		printf("%d %f %.8f\n", numberofunempty, myrows * mycols, result);
		cv::waitKey();

		return result;
	}
};
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>

#include <pcl/point_types.h>
#include <pcl/registration/icp.h>

#include <iostream>
#include <cstdio>
#include <algorithm>
#include <cstring>
#include <cmath>
#define reg register
using namespace std;
typedef long long ll;
int n, tot, T;
const int N = 110;
struct node
{
	float x, y;
} a[N], p[N];
float ans;
float m(node a, node b, node c) { return (a.x - c.x) * (b.y - c.y) - (b.x - c.x) * (a.y - c.y); }
float dis(node a, node b) { return sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y)); }
bool cmp(node p, node p2)
{
	float res = m(p, p2, a[1]);
	if (res > 0)
		return 1;
	if (res == 0 && dis(a[1], p) < dis(a[1], p2))
		return 1;
	return 0;
}

int main(int c, char *v[])
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>(5, 1));
	pcl::io::loadPCDFile<pcl::PointXYZ>(string(v[1]) + "/plant.pcd", *cloud_in);
	std::cout << "Read " << cloud_in->size() << " data points to input:" << std::endl;

	Contor conobject;
	float area_02 = conobject.statisGrids(*cloud_in, string(v[1]), atof(v[2]));
	cout << "格网法计算的面积\n"
		 << area_02 << endl;

	return (0);
}
